/*
 * ladrc.c
 *
 *  Created: 20250605
 *  Author: Hecate
 */
#include "ladrc.h"

#include <math.h>
#include <stdlib.h>

struct TrackingDifferentiator
{
    float r;
    float h;
    float h0;
    float x1;
    float x2;
    float firstFlag;
};

struct LADRC
{
    TrackingDifferentiator *td;
    float target;
    float esoBeta1;
    float esoBeta2;
    float esoBeta3;
    float esoZ1;
    float esoZ2;
    float esoZ3;
    float delta;
    float b;
    float lsefKp;
    float lsefKd;
    float controlValue;
};

static inline int sign(float x)
{
    return (x > 0) ? 1 : (x < 0) ? -1
                                 : 0;
}

static inline float fhan(float x1, float x2, float r, float h)
{
    float d = r * h * h;
    float a0 = h * x2;
    float y = x1 + a0;
    float a1 = sqrtf(d * (d + 8.0f * fabsf(y)));
    float a2 = a0 + sign(y) * (a1 - d) * 0.5;
    float a = (a0 + y) * (sign(y + d) - sign(y - d)) * 0.5f + a2 * (1 - (sign(y + d) - sign(y - d)) * 0.5f);

    return -r * (a / d) * (sign(a + d) - sign(a - d)) * 0.5f - r * sign(a) * (1.0f - (sign(a + d) - sign(a - d)) * 0.5f);
}

static inline float fal(float e, float alfa, float delta)
{
    if (fabsf(e) > delta)
        return powf(fabsf(e), alfa) * sign(e);
    else
        return e / powf(delta, 1.0f - alfa);
}

TrackingDifferentiator *trackingDifferentiatorInit(float r, float h, float n)
{
    TrackingDifferentiator *obj = (TrackingDifferentiator *) malloc(sizeof(TrackingDifferentiator));

    obj->firstFlag = 1;

    obj->r = r;
    obj->h = h;

    obj->h0 = n * h;

    obj->x1 = 0;
    obj->x2 = 0;

    return obj;
}

void trackingDifferentiatorProcess(TrackingDifferentiator *obj, float v)
{
    if (obj->firstFlag)
    {
        obj->x1 = v;
        obj->firstFlag = 0;
    }

    obj->x1 = obj->x1 + obj->h * obj->x2;
    obj->x2 = obj->x2 + obj->h * fhan(obj->x1 - v, obj->x2, obj->r, obj->h0);
}

float trackingDifferentiatorGetX1(const TrackingDifferentiator *obj)
{
    return obj->x1;
}

float trackingDifferentiatorGetX2(const TrackingDifferentiator *obj)
{
    return obj->x2;
}

LADRC *ladrcInit(float target, float r, float h, float n, float omega, float delta, float b, float kp, float kd)
{
    LADRC *obj = (LADRC *) malloc(sizeof(LADRC));

    obj->target = target;

    obj->td = trackingDifferentiatorInit(r, h, n);

    obj->esoBeta1 = 3.0f * omega;
    obj->esoBeta2 = 3.0f * omega * omega;
    obj->esoBeta3 = omega * omega * omega;

    obj->delta = delta;

    obj->esoZ1 = 0;
    obj->esoZ2 = 0;
    obj->esoZ3 = 0;

    obj->b = b;
    obj->lsefKp = kp;
    obj->lsefKd = kd;
    obj->controlValue = 0;

    return obj;
}

void ladrcSetTarget(LADRC *obj, float target)
{
    obj->target = target;
}

void ladrcProcess(LADRC *obj, float y)
{
    if (obj->td->firstFlag)
    {
        obj->td->x1 = y;
        obj->esoZ1 = y;
        obj->td->firstFlag = 0;
    }

    // td
    trackingDifferentiatorProcess(obj->td, obj->target);

    // eso
    float e = obj->esoZ1 - y;

    obj->esoZ1 = obj->esoZ1 + obj->td->h * (obj->esoZ2 - obj->esoBeta1 * e);
    obj->esoZ2 = obj->esoZ2 + obj->td->h * (obj->esoZ3 - obj->esoBeta2 * fal(e, 0.5f, obj->delta) + obj->b * obj->controlValue);
    obj->esoZ3 = obj->esoZ3 - obj->td->h * obj->esoBeta3 * fal(e, 0.25f, obj->delta);

    // lsef
    float e1 = obj->td->x1 - obj->esoZ1;

    float e2 = obj->td->x2 - obj->esoZ2;

    obj->controlValue = obj->lsefKp * e1 + obj->lsefKd * e2 - obj->esoZ3 / obj->b;
}

float ladrcGetControlValue(const LADRC *obj)
{
    return obj->controlValue;
}
